旋转的表示

反对称矩阵(Skew-symmetric Matrix)

两个三维列向量$\boldsymbol{V}_{1} = [V_{1x} V_{1y} V_{1z}]^T$和$\boldsymbol{V}_{2} = [V_{2x} V_{2y} V_{2z}]^T$之间的叉乘(外积)可用行列式计算规则表示:

其中$i,j,k$分别为直角坐标系三个坐标轴向的单位矢量。写成矩阵的形式:

称左边的特殊矩阵:

为反对称矩阵,即满足$(\boldsymbol{V}_{\times}) = - {(\boldsymbol{V}_{\times})}^T$。因此两向量之间叉乘运算可以等价为前一向量的反对称矩阵和后一向量之间的矩阵乘法运算:

反对称矩阵有如下常用的性质:

幂方公式

即:

反交换律

罗德里格斯(Rodrigues)旋转公式

假设三维空间矢量$\boldsymbol{r}$绕另一单位矢量$\boldsymbol{u}$转动$\phi \geqslant 0$角度,得矢量$\boldsymbol{r}^{\prime}$.

设矢量$\boldsymbol{r}$和单位矢量$\boldsymbol{u}$具有共同起始点$O$,记$\boldsymbol{r}$的矢端$A$在$\boldsymbol{u}$上的投影为$O^{\prime}$.以$O^{\prime}$为圆心$O^{\prime}A$为半径作圆,使$\boldsymbol{r}^{\prime}$的矢端$A^{\prime}$也在圆周上。在圆上取一点$B$使得$O^{\prime}B \perp O^{\prime}A$则有:

转动前的矢量$\boldsymbol{r}$相对于单位矢量$\boldsymbol{u}$可分解为平行于$\boldsymbol{u}$的分量$\boldsymbol{r}_{|}$和垂直分量$\boldsymbol{r}_{\perp}$.

其中:

同理,转动后的矢量$\boldsymbol{r}^{\prime}$相对于$\boldsymbol{u}$也可以分解为平行分量和垂直分量:

其中:

将1.2.3 和1.2.5 带入1.2.4得:

由三重矢积公式$(\boldsymbol{V} \cdot \boldsymbol{V_3})\boldsymbol{V} = \boldsymbol{V} \times(\boldsymbol{V} \times \boldsymbol{V}_3) + v^2 \boldsymbol{V}_3$得:

将1.2.7带入1.2.6得:

记:

称1.2.8为罗德里格斯(Rodrigues)旋转公式。它建立了旋转前后矢量$\boldsymbol{r}$和$\boldsymbol{r}^{\prime}$之间的线性变换关系,该变换是转轴$\boldsymbol{u}$及转角$\phi$的函数。

进一步,若记$\boldsymbol{\phi} = \phi \boldsymbol{u}$和$\phi = |\boldsymbol{\phi}| $则$\boldsymbol{u} = \boldsymbol{\phi}/\phi$带入1.2.9得

其中$\boldsymbol{\phi}$称为等效旋转矢量(equipment rotation vector),其矢量方向表示转轴方向,而模值大小表示旋转角度大小。从转动的物理含义上看,$(\phi\pm 2k\pi)\boldsymbol{u} (k = 0,1, \cdots)$表示的是相同的转动。

旋转矩阵

在3D空间中,旋转变换是一种线性变换,因此旋转前后保证了向量的长度以及向量之间的角度,因此旋转变化是刚体运动。

对于任意向量$\mathbf{v} \in \Bbb{R}^3$, 通过旋转操作$r()$满足以下性质:

Rotation preserves the vector norm

Rotation preserves angles between vectors

Rotation preserves the relative orientations of vectors

因此:

可得到$\mathbf{R}$为正交矩阵:

对于正交矩阵每列向量$\mathbf{R} = [\mathbf{r}_1, \mathbf{r}_2, \mathbf{r}_3]$均为单位长度并且相互正交:

因此旋转矩阵具有一下性质 :

四元数(Hamilton)

四元数定义成实部+虚部的形式(scalar + vector):

因此表示一个四元数为:

其中:

乘法

四元数乘法不满足交换律(not commutative):

满足结合律(associative):

满足加法分配率(distributive):

对于四元数乘法可以写成矩阵的形式:

其中:

因此:

四元数表示旋转

类似复数的三角表示法,四元数也可以表示为三角函数的形式:

特别的当$|\mathbf{q}| = 1$时,即对单位四元数:

其中$\mathbf{u}$为单位长度三维矢量,表示旋转轴,即$\mathbf{u}^T\mathbf{u} = 1$,$\theta$表示轴角。因此$q_0^2 + \mathbf{q}^T\mathbf{q} = 1$为单位四元数。

四元数与旋转矩阵的关系

由1.2.9,并将$cos \theta/2 = q_w \quad \mathbf{u} sin \theta/2 = \mathbf{q}_v$带入。得:

建立了单位四元数与方向余弦矩阵之间的关系。

设一个三维矢量$\mathbf{r}$,在动坐标系(b 系)和参考坐标系(i 系)的投影坐标分别为$\mathbf{r}^b$和$\mathbf{r}^i$, 先对矢量$\mathbf{r}^b$(零标量四元数)实施四元数乘法操作:

可定义四元数与三维矢量的乘法运算为坐标旋转公式。

Double Cover

四元数与3D旋转并不是一一对应的,同一个3D旋转可以使用两个四元数表示,对任意单位四元数$q = [cons(\theta/2),sin(\theta/2)\boldsymbol{u}]$,$q$与$-q$表示的是用一个旋转,分别表示绕着旋转轴$\boldsymbol{u}$旋转$\theta$角度和绕着旋转轴$-\boldsymbol{u}$旋转$2\pi - \theta$角度:

由四元数旋转公式:

因此两个四元数表示的旋转等价,称为双倍覆盖(double cover)。

四元数微分方程

对于连续形式下的旋转状态递推:

由四元数旋转公式,当旋转一段微小时间(t),即角度趋向于0时($sin\theta \approx \theta \quad cos\theta \approx 1$),得:

角速度:

因此四元数微分方程:

常用四元数求导案例(右乘扰动方式)

  • $f(q^w_b) = q^w_b * v^b$

  • $f(q^w_b) = (q^w_b)^{-1} * v^w$

李群李代数

SO(3)的伴随性质


Reference

[1] <捷联惯导算法与组合导航原理讲义>
[2] <视觉SLAM十四讲>
[3] Quaternion kinematics for the error-state Kalman filter
[4] A micro Lie theory for state estimation in robotics
[5] https://opensource.docs.anymal.com/doxygen/kindr/master/cheatsheet_latest.pdf
[6] https://quaternions.online/|
[7] https://eater.net/quaternions/
[8] https://www.bilibili.com/video/av33385105
[] https://www.zhihu.com/question/23005815?sort=created
[] http://danceswithcode.net/engineeringnotes/rotations_in_3d/demo3D/rotations_in_3d_tool.html